/*
 * Design Assignment 6
 *
 * Created: 3/30/2014 9:32:35 PM
 *  Author: Vincent
 */ 

#define F_CPU 1000000UL		//1Mhz clock
#include <util/delay.h>
#include <avr/io.h>

void initADC(void)
{
	ADCSRA |= (1<<ADEN)|(1<<ADPS0)|(1<<ADPS1)|(1<<ADPS2);	//make ADC enable and select clk/128
	ADMUX |= (1<<REFS0)|(1<<ADLAR);	// ADC Channel 0 Selected, data will be left justified with reference voltage same as VCC
}

uint16_t readADC3(void)
{
	//PC0(ADC0) should be 0(INPUT)
	ADCSRA |= (1<<ADSC);			//start conversion
	while((ADCSRA&(1<<ADIF))==0);	//wait for end of conversion
	return ADC;
}

void exec_stepper(uint16_t delay)
{
	//execute stepper motor control, in increments of 10
	if (delay < 0b0000110011001100)
	{
			PORTD = 0x01;
			_delay_ms(5);
			PORTD = 0x02;
			_delay_ms(5);
			PORTD = 0x04;
			_delay_ms(5);
			PORTD = 0x08;
			_delay_ms(5);
	}
	else if (delay < 0b0010010010100100)
	{
			PORTD = 0x01;
			_delay_ms(15);
			PORTD = 0x02;
			_delay_ms(15);
			PORTD = 0x04;
			_delay_ms(15);
			PORTD = 0x08;
			_delay_ms(15);
	} 
	else if (delay < 0b0100000000000000)
	{
		PORTD = 0x01;
		_delay_ms(25);
		PORTD = 0x02;
		_delay_ms(25);
		PORTD = 0x04;
		_delay_ms(25);
		PORTD = 0x08;
		_delay_ms(25);
	}
		else if (delay < 0b0101100110011001)
	{
			PORTD = 0x01;
			_delay_ms(35);
			PORTD = 0x02;
			_delay_ms(35);
			PORTD = 0x04;
			_delay_ms(35);
			PORTD = 0x08;
			_delay_ms(35);
	} 
	else if (delay < 0b0111001100110011)
	{
		PORTD = 0x01;
		_delay_ms(45);
		PORTD = 0x02;
		_delay_ms(45);
		PORTD = 0x04;
		_delay_ms(45);
		PORTD = 0x08;
		_delay_ms(45);
	}
	else if (delay < 0b1000110011001100)
	{
		PORTD = 0x01;
		_delay_ms(55);
		PORTD = 0x02;
		_delay_ms(55);
		PORTD = 0x04;
		_delay_ms(55);
		PORTD = 0x08;
		_delay_ms(55);
	}
	else if (delay < 0b1010011001100110)
	{
		PORTD = 0x01;
		_delay_ms(65);
		PORTD = 0x02;
		_delay_ms(65);
		PORTD = 0x04;
		_delay_ms(65);
		PORTD = 0x08;
		_delay_ms(65);
	}
	else if (delay < 0b1100000000000000)
	{
		PORTD = 0x01;
		_delay_ms(75);
		PORTD = 0x02;
		_delay_ms(75);
		PORTD = 0x04;
		_delay_ms(75);
		PORTD = 0x08;
		_delay_ms(75);
	}
	else if (delay < 0b1101100110011001)
	{
		PORTD = 0x01;
		_delay_ms(85);
		PORTD = 0x02;
		_delay_ms(85);
		PORTD = 0x04;
		_delay_ms(85);
		PORTD = 0x08;
		_delay_ms(85);
	}
	else
	{
		PORTD = 0x01;
		_delay_ms(95);
		PORTD = 0x02;
		_delay_ms(95);
		PORTD = 0x04;
		_delay_ms(95);
		PORTD = 0x08;
		_delay_ms(95);
	}
}

void exec_servo(uint16_t delay)
{
	//execute servo motor pwm control, in increments of 10
	if (delay < 0b0000110011001100)
		OCR1A = 31;
	else if (delay < 0b0010010010100100)
		OCR1A = 93;
	else if (delay < 0b0100000000000000)
		OCR1A = 156;
	else if (delay < 0b0101100110011001)
		OCR1A = 218;
	else if (delay < 0b0111001100110011)
		OCR1A = 280;
	else if (delay < 0b1000110011001100)
		OCR1A = 343;
	else if (delay < 0b1010011001100110)
		OCR1A = 405;
	else if (delay < 0b1100000000000000)
		OCR1A = 468;
	else if (delay < 0b1101100110011001)
		OCR1A = 530;
	else
		OCR1A = 592;
}


int main(void)
{
//PC0 will control speed of stepper motor and angle or servo motor
	DDRC = 0x00;				//Control Port
	DDRD = 0xFF;				//Signal Output port for stepper motor
	DDRB |= (1<<PB1);			//Set PORTB1 pin(OC1A) as output(for stepper motor)
	initADC();					//initialize A2D conversion

	//Servo motor command initialization
	ICR1=2499;									//defines top(period), 50Hz PWM, Fpwm = (Fcpu)/(PreScaler*(1+TOP))
	TCCR1A|=(1<<COM1A1)|(1<<WGM11);				//Non inverting mode, clear on match, set on top
	TCCR1B|=(1<<WGM13)|(1<<WGM12)|(1<<CS11);	//fast PWM mode, 8 bit, prescaler = 8

	while(1)
	{
		uint16_t tempval = readADC3();	//read pot value, this data should range from 0 to 65,536 (16-bit)
		exec_servo(tempval);		//execute servo motor control based on analog reading
		exec_stepper(tempval);	//execute stepper motor control based on analog reading
	}	
}

//PC0 will control speed of stepper motor, and servo motor
//PORTD  will send signals to stepper motor
//PB1 will send signals to servo motor
